#include "user_device.h"
#include "systick.h"
#include "usart.h"
#include "modbus.h"
#include "itr.h"
#include "ms35711.h"
#include "tim.h"
#include "speed_prot.h"

unsigned char server_address;

UU16 system_tick = {0};

void Read_Address(void)
{
    server_address = 0x0B;
}

ushort SystemTickLow()
{
    ushort tick = timer_counter_read(TIMER6);
    return tick;
}

ushort SystemTickHigh()
{
    uchar hi, lo;

    do
    {
        hi = system_tick.UU8[MSB];
        lo = system_tick.UU8[LSB];
    }
    while(hi != system_tick.UU8[MSB]);

    return (ushort)hi << 8 | lo;
}

ulong SystemTick()
{
    ushort hi, lo;
    do
    {
        hi = SystemTickHigh();
        lo = SystemTickLow();
    }
    while(hi != SystemTickHigh());

    return (ulong)hi << 16 | lo;
}

void Delay_Tick(ushort tick)
{
    ushort t = SystemTickLow();
    while((ushort)(SystemTickLow() - t) < tick);
}

void Delay_Tick_Loop(ulong tick)
{
    ulong t = SystemTick();
    while((ulong)(SystemTick() - t) < tick)
    {
        Main_Loop();
    }
}

void Delay_ms(uint32_t count)
{
   Delay_Tick_Loop(count*1000);
}

void Device_Init(void)
{
    systick_config();
    MX_TIM5_Init();
    MX_TIM6_Init();
    usart0_Config();
    usart2_Config();
    
    exti_itr();
    ms35711XY_init(1,64,3,0);
    STEP_X_config();
    STEP_Y_config();
    Read_Address();
    is_left_side = (server_address == 0x0B);
    *(char*)slave_flag &= ~0x07;
    #ifdef IGNORE_Z
            slave_flag = 0;
    #else
            if (is_left_side)
                    *(char*)slave_flag |= 0x03;
            else
                    *(char*)slave_flag |= 0x07;
    #endif
    register_init();
}
